A Partially Fixed Linearization Approach for Submap-Parametrized Smoothing and Mapping

We present an extension of a smoothing approach to Simultaneous Localization and Mapping (SLAM). We have previously introduced Square-Root SAM, a Smoothing and Mapping approach to SLAM based on Levenberg-Marquardt (LM) optimization. It iteratively finds the optimal nonlinear least squares solution (ML), where one iteration comprises of a linearization step, a matrix factorization, and a backsubstitution step. We introduce a submap parametrization which enables a rigid transformation of parts relative to each other during the optimization process. This parameterization is used in a multifrontal QR factorization approach, in which we partially fix the linearization point for a subset of the unknowns corresponding to sub-maps. This greatly accelerates the optimization of an entire SAM graph yet yields an exact solution.